package edu.tfh.s2.ehkbot.gerete;

import java.util.Vector;

import edu.tfh.s2.ehkbot.daten.AdressenDispatcher;
import edu.tfh.s2.ehkbot.daten.Einstellungen;
import edu.tfh.s2.ehkbot.daten.InitFactory;
import edu.tfh.s2.ehkbot.gerete.aktoren.MotorAktor;
import edu.tfh.s2.ehkbot.gerete.sensoren.Sensor;
import edu.tfh.s2.ehkbot.steuerung.Richtung;
import edu.tfh.s2.ehkbot.util.Subject;

/**
 * Fahrgestell ist die Basis für den Arm und die Räder.
 * 
 * @author s2zehn
 */
public class Fahrgestell extends Subject {
	/**
	 * FahrBefehl speicher Befehle die an das Fahrgestell übergeben werden.
	 */
	public class FahrBefehl {
		private Richtung dreheBefehl;
		private Long fahreBefehl;

		/**
		 * Konstruktor für ein Fahrbefehl.
		 * 
		 * @param fahreBefehl
		 *            Der Fahrbefehl oder fals kein Fahrbefehl vorliegt null.
		 * @param dreheBefehl
		 *            Der Drehbefehl oder fals kein Drehbefehl vorliegt null.
		 */
		public FahrBefehl(Long fahreBefehl, Richtung dreheBefehl) {
			this.fahreBefehl = fahreBefehl;
			this.dreheBefehl = dreheBefehl;
		}

		/**
		 * Liefert den Letzten Drehbefehl.
		 * 
		 * @return Die Richtung bzw .
		 */
		public Richtung getDreheBefehl() {
			return dreheBefehl;
		}

		/**
		 * Liefert den Letzten Fahrbefehl.
		 * 
		 * @return Die Strecke.
		 */
		public Long getFahreBefehl() {
			return fahreBefehl;
		}

		/**
		 * Prüft ob dieser Befehl ein Drehbefehl ist.
		 * 
		 * @return true fals es ein Drehbefehl ist.
		 */
		public boolean istDrehBefehl() {
			return dreheBefehl != null;
		}

		/**
		 * Prüft ob dieser Befehl ein Fahrbefehl ist.
		 * 
		 * @return true fals es ein Fahrbefehl ist.
		 */
		public boolean istFahrBefehl() {
			return fahreBefehl != null;
		}
	}

	public static final String modulName = "Fahrgestell";

	private Vector<Sensor> abstandsSensoren;
	private Vector<FahrBefehl> befehlsLogbuch;
	private MotorAktor linkerMotor;
	private MotorAktor rechterMotor;

	/**
	 * Konstrukot von Fahrgestell.
	 */
	public Fahrgestell() {
		befehlsLogbuch = new Vector<FahrBefehl>();
		abstandsSensoren = new Vector<Sensor>();

		InitFactory initf = InitFactory.getInstance();
		AdressenDispatcher adresD = AdressenDispatcher.getInstance();
		this.linkerMotor = initf.getMotorAktor(adresD.getAdresseFor(modulName, "Links"));
		this.rechterMotor = initf.getMotorAktor(adresD.getAdresseFor(modulName, "Rechts"));

		/*
		 * 000 045 sind angaben in grad, in diese richtungen schauen die sensen, diese angeben sind natürlich
		 * nicht absolut zum geschäft sondern zum bot
		 */
		this.abstandsSensoren.add(initf.getSensor(adresD.getAdresseFor(modulName, "000")));
		this.abstandsSensoren.add(initf.getSensor(adresD.getAdresseFor(modulName, "045")));
		this.abstandsSensoren.add(initf.getSensor(adresD.getAdresseFor(modulName, "O90")));
		this.abstandsSensoren.add(initf.getSensor(adresD.getAdresseFor(modulName, "135")));
		this.abstandsSensoren.add(initf.getSensor(adresD.getAdresseFor(modulName, "180")));
		this.abstandsSensoren.add(initf.getSensor(adresD.getAdresseFor(modulName, "225")));
		this.abstandsSensoren.add(initf.getSensor(adresD.getAdresseFor(modulName, "270")));
		this.abstandsSensoren.add(initf.getSensor(adresD.getAdresseFor(modulName, "315")));
	}

	/**
	 * Läst den Bot auf der Steller drehen um X grad (relativ zur jetzigen Richtung).
	 * 
	 * @param grad
	 *            Der Betrag um den gedreht werden soll.
	 */
	public void dreheBot(Richtung grad) {
		if (grad.getAlsRad() == 0.0)
			return;

		double radRaduis = Einstellungen.getInstance().getDoublePropertie("RadRadius");
		double radAbstand = Einstellungen.getInstance().getDoublePropertie("RadAbstand");

		double kreisUmfang = 2 * Math.PI * (radAbstand / 2);
		double zuFahren = kreisUmfang * (2 * Math.PI / Math.abs(grad.getAlsRad()));

		double radUmfang = 2 * Math.PI * radRaduis;
		double zuDrehen = (zuFahren / radUmfang) * MotorAktor.UMDREHUNG;

		linkerMotor.dreheRechts(zuDrehen);
		rechterMotor.dreheLinks(zuDrehen);

		linkerMotor.waitUntilDone();
		rechterMotor.waitUntilDone();

		befehlsLogbuch.add(new FahrBefehl(null, grad));

		super.setState(this);
	}

	/**
	 * Läst den Bot gradeaus fahren.
	 * 
	 * @param entfernungMm
	 *            Die entfernung die Zurück gelegt werden soll.
	 */
	public void fahreBot(long entfernungMm) {
		if (entfernungMm == 0)
			return;
		double radRaduis = Einstellungen.getInstance().getDoublePropertie("RadRadius");

		double radUmfang = 2 * Math.PI * radRaduis;
		double zuDrehen = (entfernungMm / radUmfang) * MotorAktor.UMDREHUNG;

		linkerMotor.drehe(zuDrehen);
		rechterMotor.drehe(zuDrehen);

		linkerMotor.waitUntilDone();
		rechterMotor.waitUntilDone();
		befehlsLogbuch.add(new FahrBefehl(entfernungMm, null));
		super.setState(this);
	}

	/**
	 * Liefert das befehls Logbuch.
	 * 
	 * @return Das Befehlslogbuch.
	 */
	public Vector<FahrBefehl> getBefehlsLogbuch() {
		return befehlsLogbuch;
	}
}